{
  VUMaths unit
  by Marek Mauder (pentar@seznam.cz)

  The contents of this file are used with permission, subject to the Mozilla
  Public License Version 1.1 (the "License"); you may not use this file except
  in compliance with the License. You may obtain a copy of the License at
  http://www.mozilla.org/MPL/MPL-1.1.html

  Software distributed under the License is distributed on an "AS IS" basis,
  WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License for
  the specific language governing rights and limitations under the License.

  Alternatively, the contents of this file may be used under the terms of the
  GNU Lesser General Public License (the  "LGPL License"), in which case the
  provisions of the LGPL License are applicable instead of those above.
  If you wish to allow use of your version of this file only under the terms
  of the LGPL License and not to allow others to use your version of this file
  under the MPL, indicate your decision by deleting  the provisions above and
  replace  them with the notice and other provisions required by the LGPL
  License.  If you do not delete the provisions above, a recipient may use
  your version of this file under either the MPL or the LGPL License.

  For more information about the LGPL: http://www.gnu.org/copyleft/lesser.html
}

{ This unit contains some mathematical functions for working
 with numbers, vectors, matrices, quaternions, ...}

unit VUMaths;

{$DEFINE USE_ASM}
{$DEFINE USE_INLINE}

{$IFDEF FPC}
  {$MODE DELPHI}
  {$UNDEF USE_ASM}
{$ENDIF}

interface


{*************************************************************************}
{                         Types and constants                             }
{*************************************************************************}


type
  { Record representing 2-dimensional float vector.}
  TVector2 = record
    case LongInt of
      0: (X: Single;
          Y: Single);
      1: (Arr: array[0..1] of Single);
  end;
  PVector2 = ^TVector2;
  TVector2Array = array[0..MaxInt div SizeOf(TVector2) - 1] of TVector2;
  PVectorArray = ^TVector2Array;

  { Record representing 3-dimensional float vector.}
  TVector3 = record
    case LongInt of
      0: (X: Single;
          Y: Single;
          Z: Single);
      1: (Arr: array[0..2] of Single);
      2: (Vec2: TVector2);
  end;
  PVector3 = ^TVector3;
  TVector3Array = array[0..MaxInt div SizeOf(TVector3) - 1] of TVector3;
  PVector3Array = ^TVector3Array;

  { Record representing 4-dimensional float vector.}
  TVector4 = record
    case LongInt of
      0: (X: Single;
          Y: Single;
          Z: Single;
          W: Single);
      1: (Arr: array[0..3] of Single);
      2: (Vec3: TVector3);
  end;
  PVector4 = TVector4;
  TVector4Array = array[0..MaxInt div SizeOf(TVector4) - 1] of TVector4;
  PVector4Array = ^TVector4Array;

  { Record representing 3x3 float matrix.}
  TMatrix3 = record
    case LongInt of
      0: (Item11, Item12, Item13: Single;
          Item21, Item22, Item23: Single;
          Item31, Item32, Item33: Single);
      1: (Arr: array[0..2, 0..2] of Single);
      2: (Singles: array[0..8] of Single);
      3: (Rows: array[0..2] of TVector3);
  end;
  PMatrix3 = ^TMatrix3;

  { Record representing 4x4 float matrix.}
  TMatrix4 = record
    case LongInt of
      0: (Item11, Item12, Item13, Item14: Single;
          Item21, Item22, Item23, Item24: Single;
          Item31, Item32, Item33, Item34: Single;
          Item41, Item42, Item43, Item44: Single);
      1: (Arr: array[0..3, 0..3] of Single);
      2: (Singles: array[0..15] of Single);
      3: (Rows: array[0..3] of TVector4);
  end;
  PMatrix4 = ^TMatrix4;

  { Record representing quaternion.}
  TQuaternion = record
    case LongInt of
      0: (X: Single;
          Y: Single;
          Z: Single;
          W: Single);
      1: (Imag: TVector3;
          Real: Single);
      2: (Vector: TVector4);
      3: (Arr: array[0..3] of Single);
  end;
  PQuaternion = ^TQuaternion;

  { Record representing 3-dimensional plane.}
  TPlane = record
    case LongInt of
      0: (A: Single;
          B: Single;
          C: Single;
          D: Single);
      1: (Arr: array[0..3] of Single);
      2: (Normal: TVector3);
   end;
  PPlane = ^TPlane;

const
  { Various mathematical constants }
  PI:        Single = 3.1415926535897932384626433832795; // PI
  PIdiv2:    Single = 1.5707963267948966192313216916398; // PI / 2
  PIdiv3:    Single = 1.0471975511965977461542144610932; // PI / 3
  PIdiv4:    Single = 0.7853981633974483096156608458198; // PI / 4
  PIdiv180:  Single = 0.0174532925199432957692369076848; // PI / 180
  D180divPI: Single = 57.295779513082320876798154814105; // 180 / PI
  Sqrt2:     Single = 1.4142135623730950488016887242097; // Sqrt(2)
  Sqrt3:     Single = 1.7320508075688772935274463415059; // Sqrt(3)
  Sqrt5:     Single = 2.2360679774997896964091736687313; // Sqrt(5)
  Sqrt10:    Single = 3.1622776601683793319988935444327; // Sqrt(10)
  SqrtPI:    Single = 1.7724538509055160272981674833411; // Sqrt(PI)
  Sqrt2PI:   Single = 2.5066282746310005024157652848110; // Sqrt(2 * PI)
  PI2:       Single = 6.2831853071795864769252867665590; // 2 * PI
  PI3:       Single = 9.4247779607693797153879301498385; // 3 * PI
  Ln2:       Single = 0.6931471805599453094172321214581; // Ln(2)
  Ln10:      Single = 2.3025850929940456840179914546844; // Ln(10)
  LnPI:      Single = 1.1447298858494001741434273513531; // Ln(PI)
  LogPI:     Single = 0.4971498726941338543512682882909; // Log10(PI)
  LogE:      Single = 0.4342944819032518276511289189166; // Log10(E)
  E:         Single = 2.7182818284590452353602874713527; // Natural constant
  One:       Single = 1.0000000000000000000000000000000; // Sqrt(11-10)
  Intfinity: Single = 1.0 / 0.0;                         // Infinity
  Nan:       Single = 0.0 / 0.0;                         // Not a number

  SingleTolerance: Single = 0.00001;

  { Empty 2-D vector.}
  Vec2Zero: TVector2 = (X: 0; Y: 0);
  { Empty 3-D vector.}
  Vec3Zero: TVector3 = (X: 0; Y: 0; Z: 0);
  { Empty 4-D vector.}
  Vec4Zero: TVector4 = (X: 0; Y: 0; Z: 0; W: 0);
  { Empty 3x3 matrix.}
  Mat3Zero: TMatrix3 = (Singles: (0, 0, 0, 0, 0, 0, 0, 0, 0));
  { Empty 4x4 matrix.}
  Mat4Zero: TMatrix4 = (Singles: (0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0));
  { Empty 4-D vector.}
  QuatZero: TQuaternion = (X: 0; Y: 0; Z: 0; W: 0);

  { Origin of 2-D coordinate system.}
  Vec2Origin: TVector2 = (X: 0; Y: 0);
  { X Axis 2-D unit vector.}
  Vec2XUnit: TVector2 = (X: 1; Y: 0);
  { Y Axis 2-D unit vector.}
  Vec2YUnit: TVector2 = (X: 0; Y: 1);
  { Negative X Axis 2-D unit vector.}
  Vec2NegXUnit: TVector2 = (X: -1; Y: 0);
  { Negative Y Axis 2-D unit vector.}
  Vec2NegYUnit: TVector2 = (X: 0; Y: -1);

  { Origin of 3-D coordinate system.}
  Vec3Origin: TVector3 = (X: 0; Y: 0; Z: 0);
  { X Axis 3-D unit vector.}
  Vec3XUnit: TVector3 = (X: 1; Y: 0; Z: 0);
  { Y Axis 3-D unit vector.}
  Vec3YUnit: TVector3 = (X: 0; Y: 1; Z: 0);
  { Z Axis 3-D unit vector.}
  Vec3ZUnit: TVector3 = (X: 0; Y: 0; Z: 1);
  { Negative X Axis 3-D unit vector.}
  Vec3NegXUnit: TVector3 = (X: -1; Y: 0; Z: 0);
  { Negative Y Axis 3-D unit vector.}
  Vec3NegYUnit: TVector3 = (X: 0; Y: -1; Z: 0);
  { Negative Z Axis 3-D unit vector.}
  Vec3NegZUnit: TVector3 = (X: 0; Y: 0; Z: -1);

  { Origin of 4-D coordinate system.}
  Vec4Origin: TVector4 = (X: 0; Y: 0; Z: 0; W: 0);
  { X Axis 4-D vector.}
  Vec4XUnit: TVector4 = (X: 1; Y: 0; Z: 0; W: 0);
  { Y Axis 4-D vector.}
  Vec4YUnit: TVector4 = (X: 0; Y: 1; Z: 0; W: 0);
  { Z Axis 4-D vector.}
  Vec4ZUnit: TVector4 = (X: 0; Y: 0; Z: 1; W: 0);
  { W 4-D vector.}
  Vec4WUnit: TVector4 = (X: 0; Y: 0; Z: 0; W: 1);
  { Negative X Axis 4-D vector.}
  Vec4NegXUnit: TVector4 = (X: -1; Y: 0; Z: 0; W: 0);
  { Negative Y Axis 4-D vector.}
  Vec4NegYUnit: TVector4 = (X: 0; Y: -1; Z: 0; W: 0);
  { Negative Z Axis 4-D vector.}
  Vec4NegZUnit: TVector4 = (X: 0; Y: 0; Z: -1; W: 0);
  { Negative W 4-D vector.}
  Vec4NegWUnit: TVector4 = (X: 0; Y: 0; Z: 0; W: -1);

  { 3x3 identity matrix.}
  Mat3Identity: TMatrix3 = (Arr: ((1, 0, 0), (0, 1, 0), (0, 0, 1)));
  { 4x4 identity matrix.}
  Mat4Identity: TMatrix4 = (Arr: ((1, 0, 0, 0), (0, 1, 0, 0), (0, 0, 1, 0),
    (0, 0, 0, 1)));
  { Identity quaternion.}
  QuatIdentity: TQuaternion = (Imag: (Arr: (0, 0, 0)); Real: 1);


{*************************************************************************}
{                         Base functions                                  }
{*************************************************************************}


{ Returns Min value from array.}
function MinFrom(const Values: array of LongInt): LongInt; overload;
function MinFrom(const Values: array of Single): Single; overload;
{ Returns Max value from array.}
function MaxFrom(const Values: array of LongInt): LongInt; overload;
function MaxFrom(const Values: array of Single): Single; overload;

{ Returns True if value is in range (inclusive).}
function InRange(const Value, Min, Max: LongInt): Boolean; overload;
function InRange(const Value, Min, Max: Single): Boolean; overload;

{ Returns sum of values.}
function SumI(const Values: array of LongInt): LongInt;
function SumF(const Values: array of Single): Single;
{ Returns sum of squares of values.}
function SumSquaresI(const Values: array of LongInt): LongInt;
function SumSquaresF(const Values: array of Single): Single;

{ Returns float division remainder.}
function ModF(const A, B: Single): Single;
{ Linearly interpolates between two float values A and B at time T.}
function Lerp(const A, B, T: Single): Single;

{ Returns -1 if Value is negative, returns 1 otherwise.}
function Sign(const Value: LongInt): ShortInt; overload;
function Sign(const Value: Single): ShortInt; overload;


{*************************************************************************}
{                      Random number functions                            }
{*************************************************************************}


{ Returns long Min <= X <= Max}
function RandRange(const Min, Max: LongInt): LongInt; overload;
function RandRange(const Min, Max: Single): Single; overload;
{ Returns value with random sign + / -}
function RandSign(const Value: LongInt): LongInt; overload;
function RandSign(const Value: Single): Single; overload;
{ Returns random value from array}
function RandFromI(const Values: array of LongInt): LongInt;
function RandFromF(const Values: array of Single): Single;


{*************************************************************************}
{                Logarithmical/Exponential functions                      }
{*************************************************************************}


{ Returns Log base 2 of X.}
function Log2(const X: Single): Single;
{ Returns Log base 10 of X.}
function Log10(const X: Single): Single;
{ Returns Log base Base of X.}
function LogN(const X, Base: Single): Single;

{ Raise two to Exp.}
function Power2(const Exponent: Single): Single;
{ Raise ten to Exp.}
function Power10(const Exponent: Single): Single;
{ Raise base to any power.}
function Power(const Base, Exponent: Single): Single;


{*************************************************************************}
{                    Trigonometric/Angle functions                        }
{*************************************************************************}


{ Adjusts angle to fit in <-PI, PI>.}
function AdjustAngle(const Angle: Single): Single;

{ Converts degrees to radians.}
function DegToRad(const Degrees: Single): Single;
{ Converts radians to degrees.}
function RadToDeg(const Radians: Single): Single;

{ Returns sine and cosine of theta angle (in radians)}
procedure SinCos(const Angle: Single; var SinF, CosF: Double); overload;
procedure SinCos(const Angle: Single; var SinF, CosF: Single); overload;

{ Returns tangent of angle.}
function Tan(const Angle: Single): Single;
{ Returns cotangent of angle.}
function CoTan(const Angle: Single): Single;

{ Returns inverse sine (in radians) of Value (between -1 and 1).}
function ArcSin(const Value: Single): Single;
{ Returns inverse cosine (in radians) of Value (between -1 and 1).}
function ArcCos(const Value: Single): Single;
{ Returns inverse tangent (in radians) of Value.}
function ArcTan(const Value: Single): Single;
{ Returns inverse cotangent (in radians) of Value.}
function ArcCoTan(const Value: Single): Single;


{*************************************************************************}
{                            Vector functions                             }
{*************************************************************************}


{ Vector creation procedures.}
procedure Vector2(const X, Y: Single; var Vec: TVector2); overload;
procedure Vector3(const X, Y, Z: Single; var Vec: TVector3); overload;
procedure Vector4(const X, Y, Z, W: Single; var Vec: TVector4); overload;
{ Vector creation functions.}
function Vector2(const X, Y: Single): TVector2; overload;
function Vector3(const X, Y, Z: Single): TVector3; overload;
function Vector3(const V: TVector2; Z: Single): TVector3; overload;
function Vector4(const X, Y, Z, W: Single): TVector4; overload;
function Vector4(const V: TVector3; W: Single): TVector4; overload;

{* 2D Vector functions *}

{ Adds vector V1 to V2.}
function Vec2Add(const V1, V2: TVector2): TVector2;
{ Subtracts vector V2 from V1.}
function Vec2Subtract(const V1, V2: TVector2): TVector2;
{ Multiplies vector V1 with V2.}
function Vec2Multiply(const V1, V2: TVector2): TVector2;
{ Divides vector V1 by V2.}
function Vec2Divide(const V1, V2: TVector2): TVector2;
{ Returns True if vectors have the same components.}
function Vec2Compare(const V1, V2: TVector2): Boolean;

{ Returns dot product of two vectors V1 and V2.}
function Vec2Dot(const V1, V2: TVector2): Single;
{ Returns length of vector V.}
function Vec2Length(const V: TVector2): Single;
{ Returns distance between vectors V1 and V2.}
function Vec2Dist(const V1, V2: TVector2): Single;
{ Returns squared distance between vectors V1 and V2.}
function Vec2DistSqr(const V1, V2: TVector2): Single;
{ Calculates linear interpolation between vectors V1 and V2 at poin T (0.0 - 1.0)}
function Vec2Lerp(const V1, V2: TVector2; const T: Single): TVector2;
{ Returns vector that is made up of the largest components of vectors V1 and V2.}
function Vec2Maximize(const V1, V2: TVector2): TVector2;
{ Returns vector that is made up of the smallest components of vectors V1 and V2.}
function Vec2Minimize(const V1, V2: TVector2): TVector2;

{ Scales vector V by scalar ScaleBy.}
procedure Vec2ScaleProc(var V: TVector2; const ScaleBy: Single);
function Vec2Scale(const V: TVector2; const ScaleBy: Single): TVector2;
{ Negates given vector V (Returns (-1, 2, -3) from input (1, -2, 3)). }
procedure Vec2NegateProc(var V: TVector2);
function Vec2Negate(const V: TVector2): TVector2;
{ Normalizes given vector V (Result will be unit length vector).}
procedure Vec2NormalizeProc(var V: TVector2);
function Vec2Normalize(const V: TVector2): TVector2;

{* 3D Vector functions *}

{ Adds vector V1 to V2.}
function Vec3Add(const V1, V2: TVector3): TVector3;
{ Subtracts vector V2 from V1.}
function Vec3Subtract(const V1, V2: TVector3): TVector3;
{ Multiplies vector V1 with V2.}
function Vec3Multiply(const V1, V2: TVector3): TVector3;
{ Divides vector V1 by V2.}
function Vec3Divide(const V1, V2: TVector3): TVector3;
{ Returns True if vectors have the same components.}
function Vec3Compare(const V1, V2: TVector3): Boolean;

{ Returns dot product of two vectors V1 and V2.}
function Vec3Dot(const V1, V2: TVector3): Single;
{ Returns cross product of two vectors V1 and V2.}
function Vec3Cross(const V1, V2: TVector3): TVector3;
{ Returns length of vector V.}
function Vec3Length(const V: TVector3): Single;
{ Returns squared length of vector V.}
function Vec3LengthSquared(const V: TVector3): Single;
{ Returns distance between vectors V1 and V2.}
function Vec3Dist(const V1, V2: TVector3): Single;
{ Returns squared distance between vectors V1 and V2.}
function Vec3DistSqr(const V1, V2: TVector3): Single; 
{ Returns squared distance between vectors V1 and V2.}
function Vec3DistSquared(const V1, V2: TVector3): Single;
{ Calculates linear interpolation between vectors V1 and V2 at time T (0.0 - 1.0)}
function Vec3Lerp(const V1, V2: TVector3; const T: Single): TVector3;
{ Returns vector that is made up of the largest components of vectors V1 and V2.}
function Vec3Maximize(const V1, V2: TVector3): TVector3;
{ Returns vector that is made up of the smallest components of vectors V1 and V2.}
function Vec3Minimize(const V1, V2: TVector3): TVector3;

{ Scales vector V by scalar ScaleBy.}
procedure Vec3ScaleProc(var V: TVector3; const ScaleBy: Single);
function Vec3Scale(const V: TVector3; const ScaleBy: Single): TVector3;
{ Negates given vector V (Returns (-1, 2, -3) from input (1, -2, 3)). }
procedure Vec3NegateProc(var V: TVector3);
function Vec3Negate(const V: TVector3): TVector3;
{ Normalizes given vector V (Result will be unit length vector).}
procedure Vec3NormalizeProc(var V: TVector3);
function Vec3Normalize(const V: TVector3): TVector3;

{ Multiplies row vector V by matrix M.}
function Vec3Transform(const V: TVector3; const M: TMatrix3): TVector3;
{ Computes average of two vectors A = (V1 + V2) / 2.} 
function Vec3Average(const V1, V2: TVector3): TVector3;

function Vec3RotateX(const V: TVector3; const Angle : Single) : TVector3;
function Vec3RotateY(const V: TVector3; const Angle : Single) : TVector3;
function Vec3RotateZ(const V: TVector3; const Angle : Single) : TVector3;

{* 4D Vector functions *}

{ Adds vector V1 to V2.}
function Vec4Add(const V1, V2: TVector4): TVector4;
{ Subtracts vector V2 from V1.}
function Vec4Subtract(const V1, V2: TVector4): TVector4;
{ Multiplies vector V1 with V2.}
function Vec4Multiply(const V1, V2: TVector4): TVector4;
{ Divides vector V1 by V2.}
function Vec4Divide(const V1, V2: TVector4): TVector4;
{ Returns True if vectors have the same components.}
function Vec4Compare(const V1, V2: TVector4): Boolean;

{ Returns dot product of two vectors V1 and V2.}
function Vec4Dot(const V1, V2: TVector4): Single;
{ Returns cross product of two vectors V1 and V2.}
function Vec4Cross(const V1, V2: TVector4): TVector4;
{ Returns length of vector V.}
function Vec4Length(const V: TVector4): Single;
{ Returns distance between vectors V1 and V2.}
function Vec4Dist(const V1, V2: TVector4): Single;
{ Calculates linear interpolation between vectors V1 and V2 at poin T (0.0 - 1.0)}
function Vec4Lerp(const V1, V2: TVector4; const T: Single): TVector4;
{ Returns vector that is made up of the largest components of vectors V1 and V2.}
function Vec4Maximize(const V1, V2: TVector4): TVector4;
{ Returns vector that is made up of the smallest components of vectors V1 and V2.}
function Vec4Minimize(const V1, V2: TVector4): TVector4;

{ Scales vector V by scalar ScaleBy.}
procedure Vec4ScaleProc(var V: TVector4; const ScaleBy: Single);
function Vec4Scale(const V: TVector4; const ScaleBy: Single): TVector4;
{ Negates given vector V (Returns (-1, 2, -3) from input (1, -2, 3)). }
procedure Vec4NegateProc(var V: TVector4);
function Vec4Negate(const V: TVector4): TVector4;
{ Normalizes given vector V (Result will be unit length vector).}
procedure Vec4NormalizeProc(var V: TVector4);
function Vec4Normalize(const V: TVector4): TVector4;

{ Multiplies row vector V by matrix M.}
function Vec4Transform(const V: TVector4; const M: TMatrix4): TVector4;


{*************************************************************************}
{                            Matrix functions                             }
{*************************************************************************}


{ Matrix creation procedures.}
procedure Matrix3(const Singles: array of Single; var M: TMatrix3); overload;
procedure Matrix3(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; var M: TMatrix3); overload;
procedure Matrix4(const Singles: array of Single; var M: TMatrix4); overload;
procedure Matrix4(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; const Row4: array of Single; var M: TMatrix4);
overload;
{ Matrix creation functions.}
function Matrix3(const Singles: array of Single): TMatrix3; overload;
function Matrix3(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single): TMatrix3; overload;
function Matrix4(const Singles: array of Single): TMatrix4; overload;
function Matrix4(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; const Row4: array of Single): TMatrix4; overload;

{* 3x3 Matrix functions *}

{ Adds 3x3 matrix M1 to M2.}
function Mat3Add(const M1, M2: TMatrix3): TMatrix3;
{ Subtracts 3x3 matrix M2 form M1.}
function Mat3Subtract(const M1, M2: TMatrix3): TMatrix3;
{ Multiplies 3x3 matrices M1 and M2.}
function Mat3Multiply(const M1, M2: TMatrix3): TMatrix3;
{ Scales all components of 3x3 matrix M by F.}
function Mat3ScalarMultiply(const M: TMatrix3; const F: Single): TMatrix3;
{ Returns True if 3x3 matrices M1 and M2 have the same components.}
function Mat3Compare(const M1, M2: TMatrix3): Boolean;

{ Transposes 3x3 matrix M.}
function Mat3Transpose(const M: TMatrix3): TMatrix3;
{ Returns determinant of 3x3 matrix M.}
function Mat3Determinant(const M: TMatrix3): Single;
{ Returns inverted 3x3 matrix M.}
function Mat3Inverse(const M: TMatrix3): TMatrix3;

{* 4x4 Matrix functions *}

{ Adds 4x4 matrix M1 to M2.}
function Mat4Add(const M1, M2: TMatrix4): TMatrix4;
{ Subtracts 4x4 matrix M2 form M1.}
function Mat4Subtract(const M1, M2: TMatrix4): TMatrix4;
{ Multiplies 4x4 matrices M1 and M2.}
function Mat4Multiply(const M1, M2: TMatrix4): TMatrix4;
{ Scales all components of 4x4 matrix M by F.}
function Mat4ScalarMultiply(const M: TMatrix4; const F: Single): TMatrix4;
{ Returns True if 4x4 matrices M1 and M2 have the same components.}
function Mat4Compare(const M1, M2: TMatrix4): Boolean;

{ Transposes 4x4 matrix M.}
function Mat4Transpose(const M: TMatrix4): TMatrix4;
{ Returns determinant of 4x4 matrix M.}
function Mat4Determinant(const M: TMatrix4): Single;
{ Returns inverted 4x4 matrix M.}
function Mat4Inverse(const M: TMatrix4): TMatrix4;


{*************************************************************************}
{                            Quaternion functions                         }
{*************************************************************************}

{ Quaternion creation functions.}
function Quaternion(const X, Y, Z, W: Single): TQuaternion; overload;
{ Converts quaternion Q to 4x4 matrix.}
function QuatToMatrix(const Q: TQuaternion): TMatrix4;

{ Multiplies quaternions Q1 and Q2.}
function QuatMultiply(const Q1, Q2: TQuaternion): TQuaternion;
{ Returns conjugate of quaternion Q.}
function QuatConjugate(const Q: TQuaternion): TQuaternion;

{*************************************************************************}
{                              Plane functions                            }
{*************************************************************************}

{ Plane creation functions.}
function Plane(const A, B, C, D: Single): TPlane; overload;
function Plane(const Normal: TVector3; const D: Single): TPlane; overload;

{ Normalizes given plane.}
procedure PlaneNormalizeProc(var Plane: TPlane);
function PlaneNormalize(const Plane: TPlane): TPlane;
{ Calculates signed distance from Point to Plane (if distance is negative point
  lies behind the plane).}
function PlaneDistanceToPoint(const Plane: TPlane; const Point: TVector3): Single;

// debug

function Vector3ToStr(const v : TVector3; digits : integer = 2) : string;
procedure Matrix4Debug(AMatrix : TMatrix4; AName : string; ADigits : integer = 2);

implementation

procedure Matrix4Debug(AMatrix : TMatrix4; AName : string; ADigits : integer = 2);
var i : integer;
begin
  writeln('Matrix4: ',AName);
  for i := 0 to 3 do
    writeln('  ',AMatrix.Arr[i,0]:1:ADigits,'  ',AMatrix.Arr[i,1]:1:ADigits,'  ',AMatrix.Arr[i,2]:1:ADigits,'  ',AMatrix.Arr[i,3]:1:ADigits);
end;

function Vector3ToStr(const v : TVector3; digits : integer = 2) : string;
var s : string;
begin
	str(v.x:1:digits,s);
	result := 'x:'+s+' ';
	str(v.y:1:digits,s);
	result := result+'y:'+s+' ';
	str(v.z:1:digits,s);
	result := result+'z:'+s;
end;

{*************************************************************************}
{                         Base functions                                  }
{*************************************************************************}


function MinFrom(const Values: array of LongInt): LongInt;
var
  I: LongInt;
begin
  Result := Values[Low(Values)];
  for I := Low(Values) + 1 to High(Values) do
    if Result > Values[I] then
      Result := Values[I];
end;

function MinFrom(const Values: array of Single): Single;
var
  I: LongInt;
begin
  Result := Values[Low(Values)];
  for I := Low(Values) + 1 to High(Values) do
    if Result > Values[I] then
      Result := Values[I];
end;

function MaxFrom(const Values: array of LongInt): LongInt;
var
  I: LongInt;
begin
  Result := Values[Low(Values)];
  for I := Low(Values) + 1 to High(Values) do
    if Result < Values[I] then
      Result := Values[I];
end;

function MaxFrom(const Values: array of Single): Single;
var
  I: LongInt;
begin
  Result := Values[Low(Values)];
  for I := Low(Values) + 1 to High(Values) do
    if Result < Values[I] then
      Result := Values[I];
end;

function InRange(const Value, Min, Max: LongInt): Boolean;
begin
  Result := (Value >= Min) and (Value <= Max);
end;

function InRange(const Value, Min, Max: Single): Boolean;
begin
  Result := (Value >= Min) and (Value <= Max);
end;

function SumI(const Values: array of LongInt): LongInt;
var
  I: LongInt;
begin
  Result := 0;
  for I := Low(Values) to High(Values) do
    Result := Result + Values[I];
end;

function SumF(const Values: array of Single): Single;
var
  I: LongInt;
begin
  Result := 0.0;
  for I := Low(Values) to High(Values) do
    Result := Result + Values[I];
end;

function SumSquaresI(const Values: array of LongInt): LongInt;
var
  I: LongInt;
begin
  Result := 0;
  for I := Low(Values) to High(Values) do
    Result := Result + Sqr(Values[I]);
end;

function SumSquaresF(const Values: array of Single): Single;
var
  I: LongInt;
begin
  Result := 0.0;
  for I := Low(Values) to High(Values) do
    Result := Result + Sqr(Values[I]);
end;

function ModF(const A, B: Single): Single;
var
  Z: Single;
begin
  Result := A / B;
  Z := Int(Result);
  if Result < 0.0 then
    Z := Z - 1.0;
  Result := A - Z * B;
end;

function Lerp(const A, B, T: Single): Single;
begin
  Result := A + (B - A) * T;
end;


function Sign(const Value: LongInt): ShortInt;
begin
  if Value < 0 then
    Result := -1
  else
    Result := 1;
end;

function Sign(const Value: Single): ShortInt;
begin
  if Value < 0 then
    Result := -1
  else
    Result := +1;
end;


{*************************************************************************}
{                      Random number functions                            }
{*************************************************************************}


function RandRange(const Min, Max: LongInt): LongInt;
begin
  if Min > Max then
    Result := Random(Min - Max + 1) + Max
  else
    Result := Random(Max - Min + 1) + Min;
end;

function RandRange(const Min, Max: Single): Single;
begin
  if Min > Max then
    Result := Random(Round(Min - Max) + 1) + Max
  else
    Result := Random(Round(Max - Min) + 1) + Min;
end;

function RandSign(const Value: LongInt): LongInt;
begin
  if Random(2) = 0 then
    Result := Value
  else
    Result := -Value;
end;

function RandSign(const Value: Single): Single;
begin
  if Random(2) = 0 then
    Result := Value
  else
    Result := -Value;
end;

function RandFromI(const Values: array of LongInt): LongInt;
begin
  Result := Values[RandRange(Low(Values), High(Values))];
end;

function RandFromF(const Values: array of Single): Single;
begin
  Result := Values[RandRange(Low(Values), High(Values))];
end;


{*************************************************************************}
{                   Logarithmical/Exponential functions                   }
{*************************************************************************}


function Log2(const X: Single): Single;
begin
  Result := Ln(X) / Ln2;
end;

function Log10(const X: Single): Single;
begin
  Result := Ln(X) / Ln10;
end;

function LogN(const X, Base: Single): Single;
begin
  Result := Ln(X) / Ln(Base);
end;

function Power2(const Exponent: Single): Single;
begin
  if Exponent = 0.0 then
    Result := 1.0
  else
    Result := Exp(Exponent * Ln2);
end;

function Power10(const Exponent: Single): Single;
begin
  if Exponent = 0.0 then
    Result := 1.0
  else
    Result := Exp(Exponent * Ln10);
end;

function Power(const Base, Exponent: Single): Single;
begin
  if Exponent = 0.0 then
    Result := 1.0
  else
    if (Base = 0.0) and (Exponent > 0.0) then
      Result := 0.0
    else
      Result := Exp(Exponent * Ln(Base))
end;


{*************************************************************************}
{                      Trigonometric/Angle functions                      }
{*************************************************************************}


function AdjustAngle(const Angle: Single): Single;
begin
  Result := Angle;
  repeat
    if Result <= -PI then
      Result := Result + 2 * PI;
    if Result > PI then
      Result := Result - 2 * PI;
  until (Result > -PI) and (Result <= PI);
end;

function DegToRad(const Degrees: Single): Single;
{$IFDEF USE_ASM}
{$IFNDEF PIC}
asm
  FLD    Degrees
  FMUL   PIdiv180
  FWAIT
end;
{$ELSE}
begin
  Result := Degrees * PIdiv180;
end;
{$ENDIF}
{$ELSE}
begin
  Result := Degrees * PIdiv180;
end;
{$ENDIF}

function RadToDeg(const Radians: Single): Single;
{$IFDEF USE_ASM}
{$IFNDEF PIC}
asm
  FLD    Radians
  FMUL   D180divPi
  FWAIT
end;
{$ELSE}
begin
  Result := Radians * D180divPI;
end;
{$ENDIF}
{$ELSE}
begin
  Result := Radians * D180divPI;
end;
{$ENDIF}

procedure SinCos(const Angle: Single; var SinF, CosF: Double);
{$IFDEF USE_ASM}
asm
  FLD    Angle
  FSINCOS
  FSTP   QWORD PTR [EDX]
  FSTP   QWORD PTR [EAX]
end;
{$ELSE}
begin
  SinF := System.Sin(Angle);
  CosF := System.Cos(Angle);
end;
{$ENDIF}

procedure SinCos(const Angle: Single; var SinF, CosF: Single);
{$IFDEF USE_ASM}
asm
  FLD    Angle
  FSINCOS
  FSTP   DWORD PTR [EDX]
  FSTP   DWORD PTR [EAX]
end;
{$ELSE}
begin
  SinF := System.Sin(Angle);
  CosF := System.Cos(Angle);
end;
{$ENDIF}

function Tan(const Angle: Single): Single;
{$IFDEF USE_ASM}
asm
  FLD    Angle
  FPTAN
  FSTP   ST(0)
end;
{$ELSE}
begin
  Result := Sin(Angle) / Cos(Angle);
end;
{$ENDIF}

function CoTan(const Angle: Single): Single;
{$IFDEF USE_ASM}
asm
  FLD    Angle
  FPTAN
  FDIVRP
end;
{$ELSE}
begin
  Result := Cos(Angle) / Sin(Angle);
end;
{$ENDIF}

function ArcSin(const Value: Single): Single;
{$IFDEF USE_ASM}
{$IFNDEF PIC}
asm
  PUSH   EBX
  FLD    Value
  FLD    ST
  FMUL   ST, ST
  FSUBR  One
  FSQRT
  FPATAN
  POP    EBX
end;
{$ELSE}
begin
  if Value < 0.0 then
    Result := -ArcSin(-Value)
  else
    if Value = 1.0 then
      Result := PI / 2
    else
      Result := ArcTan(Value / Sqrt(1 - Value * Value));
end;
{$ENDIF}
{$ELSE}
begin
  if Value < 0.0 then
    Result := -ArcSin(-Value)
  else
    if Value = 1.0 then
      Result := PI / 2
    else
      Result := ArcTan(Value / Sqrt(1 - Value * Value));
end;
{$ENDIF}

function ArcCos(const Value: Single): Single;
{$IFDEF USE_ASM}
{$IFNDEF PIC}
asm
  FLD    Value
  FMUL   ST, ST
  FSUBR  One
  FSQRT
  FLD    Value
  FPATAN
end;
{$ELSE}
begin
  if Value < 0.0 then
    Result := PI - ArcCos(-Value)
  else
    if Value = 0.0 then
      Result := PI / 2
    else
      Result := ArcTan(Sqrt(1 - Value * Value) / Value);
end;
{$ENDIF}
{$ELSE}
begin
  if Value < 0.0 then
    Result := PI - ArcCos(-Value)
  else
    if Value = 0.0 then
      Result := PI / 2
    else
      Result := ArcTan(Sqrt(1 - Value * Value) / Value);
end;
{$ENDIF}

function ArcTan(const Value: Single): Single;
{$IFDEF USE_ASM}
asm
  FLD    Value
  FLD1
  FPATAN
  FWAIT
end;
{$ELSE}
begin
  Result:=System.ArcTan(Value);
end;
{$ENDIF}

function ArcCoTan(const Value: Single): Single;
{$IFDEF USE_ASM}
asm
  FLD1
  FLD    Value
  FPATAN
  FWAIT
end;
{$ELSE}
begin
 Result := ArcTan(1 / Value);
end;
{$ENDIF}


{*************************************************************************}
{                            Vector functions                             }
{*************************************************************************}


procedure Vector2(const X, Y: Single; var Vec: TVector2);
begin
  Vec.X := X;
  Vec.Y := Y;
end;

procedure Vector3(const X, Y, Z: Single; var Vec: TVector3);
begin
  Vec.X := X;
  Vec.Y := Y;
  Vec.Z := Z;
end;

procedure Vector4(const X, Y, Z, W: Single; var Vec: TVector4);
begin
  Vec.X := X;
  Vec.Y := Y;
  Vec.Z := Z;
  Vec.W := W;
end;

function Vector2(const X, Y: Single): TVector2;
begin
  Result.X := X;
  Result.Y := Y;
end;

function Vector3(const X, Y, Z: Single): TVector3;
begin
  Result.X := X;
  Result.Y := Y;
  Result.Z := Z;
end;

function Vector3(const V: TVector2; Z: Single): TVector3;
begin
  Result.Vec2 := V;
  Result.Z := Z;
end;

function Vector4(const X, Y, Z, W: Single): TVector4;
begin
  Result.X := X;
  Result.Y := Y;
  Result.Z := Z;
  Result.W := W;
end;

function Vector4(const V: TVector3; W: Single): TVector4;
begin
  Result.Vec3 := V;
  Result.W := W;
end;

{* 2D Vector functions *}

function Vec2Add(const V1, V2: TVector2): TVector2;
begin
  Result.X := V1.X + V2.X;
  Result.Y := V1.Y + V2.Y;
end;

function Vec2Subtract(const V1, V2: TVector2): TVector2;
begin
  Result.X := V1.X - V2.X;
  Result.Y := V1.Y - V2.Y;
end;

function Vec2Multiply(const V1, V2: TVector2): TVector2;
begin
  Result.X := V1.X * V2.X;
  Result.Y := V1.Y * V2.Y;
end;

function Vec2Divide(const V1, V2: TVector2): TVector2;
begin
  if V2.X <> 0 then Result.X := V1.X / V2.X else Result.X := 0;
  if V2.Y <> 0 then Result.Y := V1.Y / V2.Y else Result.Y := 0;
end;

function Vec2Compare(const V1, V2: TVector2): Boolean;
begin
  Result :=
    (Abs(V1.X - V2.X) < SingleTolerance) and
    (Abs(V1.Y - V2.Y) < SingleTolerance);
end;

function Vec2Dot(const V1, V2: TVector2): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FMUL   V2.X
  FLD    V1.Y
  FMUL   V2.Y
  FADDP
end;
{$ELSE}
begin
  Result := V1.X * V2.X + V1.Y * V2.Y;
end;
{$ENDIF}

function Vec2Length(const V: TVector2): Single;
begin
  Result := Sqrt(V.X * V.X + V.Y * V.Y);
end;

function Vec2Dist(const V1, V2: TVector2): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FSUB   V2.X
  FMUL   ST, ST
  FLD    V1.Y
  FSUB   V2.Y
  FMUL   ST, ST
  FADD
  FSQRT
end;
{$ELSE}
begin
  Result := Sqrt((V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y));
end;
{$ENDIF}

function Vec2DistSqr(const V1, V2: TVector2): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FSUB   V2.X
  FMUL   ST, ST
  FLD    V1.Y
  FSUB   V2.Y
  FMUL   ST, ST
  FADD
end;
{$ELSE}
begin
  Result := (V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y);
end;
{$ENDIF}

function Vec2Lerp(const V1, V2: TVector2; const T: Single): TVector2;
begin
  Result.X := V1.X + T * (V2.X - V1.X);
  Result.Y := V1.Y + T * (V2.Y - V1.Y);
end;

function Vec2Maximize(const V1, V2: TVector2): TVector2;
begin
  Result := V1;
  if V2.X > Result.X then
    Result.X := V2.X;
  if V2.Y > Result.Y then
    Result.Y := V2.Y;
end;

function Vec2Minimize(const V1, V2: TVector2): TVector2;
begin
  Result := V1;
  if V2.X < Result.X then
    Result.X := V2.X;
  if V2.Y < Result.Y then
    Result.Y := V2.Y;
end;

procedure Vec2ScaleProc(var V: TVector2; const ScaleBy: Single);
begin
  V.X := V.X * ScaleBy;
  V.Y := V.Y * ScaleBy;
end;

function Vec2Scale(const V: TVector2; const ScaleBy: Single): TVector2;
begin
  Result.X := V.X * ScaleBy;
  Result.Y := V.Y * ScaleBy;
end;

procedure Vec2NegateProc(var V: TVector2);
begin
  V.X := -V.X;
  V.Y := -V.Y;
end;

function Vec2Negate(const V: TVector2): TVector2;
begin
  Result.X := -V.X;
  Result.Y := -V.Y;
end;

procedure Vec2NormalizeProc(var V: TVector2);
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   V.X
  FMUL   V.Y
  FSTP   V.Y
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    V.X := V.X * InvLen;
    V.Y := V.Y * InvLen;
  end
  else
    V := Vec2Zero;
end;
{$ENDIF}

function Vec2Normalize(const V: TVector2): TVector2;
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   Result.X
  FMUL   V.Y
  FSTP   Result.Y
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    Result.X := V.X * InvLen;
    Result.Y := V.Y * InvLen;
  end
  else
    Result := Vec2Zero;
end;
{$ENDIF}

{* 3D Vector functions}

function Vec3Add(const V1, V2: TVector3): TVector3;
begin
  Result.X := V1.X + V2.X;
  Result.Y := V1.Y + V2.Y;
  Result.Z := V1.Z + V2.Z;
end;

function Vec3Subtract(const V1, V2: TVector3): TVector3;
begin
  Result.X := V1.X - V2.X;
  Result.Y := V1.Y - V2.Y;
  Result.Z := V1.Z - V2.Z;
end;

function Vec3Multiply(const V1, V2: TVector3): TVector3;
begin
  Result.X := V1.X * V2.X;
  Result.Y := V1.Y * V2.Y;
  Result.Z := V1.Z * V2.Z;
end;

function Vec3Divide(const V1, V2: TVector3): TVector3;
begin
  if V2.X <> 0 then Result.X := V1.X / V2.X else Result.X := 0;
  if V2.Y <> 0 then Result.Y := V1.Y / V2.Y else Result.Y := 0;
  if V2.Z <> 0 then Result.Z := V1.Z / V2.Z else Result.Z := 0;
end;

function Vec3Compare(const V1, V2: TVector3): Boolean;
begin
  Result :=
    (Abs(V1.X - V2.X) < SingleTolerance) and
    (Abs(V1.Y - V2.Y) < SingleTolerance) and
    (Abs(V1.Z - V2.Z) < SingleTolerance);
end;

function Vec3Dot(const V1, V2: TVector3): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FMUL   V2.X
  FLD    V1.Y
  FMUL   V2.Y
  FADDP
  FLD    V1.Z
  FMUL   V2.Z
  FADDP
end;
{$ELSE}
begin
  Result := V1.X * V2.X + V1.Y * V2.Y + V1.Z * V2.Z;
end;
{$ENDIF}

function Vec3Cross(const V1, V2: TVector3): TVector3;
begin
  Result.X := V1.Y * V2.Z - V1.Z * V2.Y;
  Result.Y := V1.Z * V2.X - V1.X * V2.Z;
  Result.Z := V1.X * V2.Y - V1.Y * V2.X;
end;

function Vec3Length(const V: TVector3): Single;
begin
  Result := Sqrt(V.X * V.X + V.Y * V.Y + V.Z * V.Z);
end;

function Vec3LengthSquared(const V: TVector3): Single;
begin
  Result := Sqrt(V.X * V.X + V.Y * V.Y + V.Z * V.Z);
end;

function Vec3Dist(const V1, V2: TVector3): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FSUB   V2.X
  FMUL   ST, ST
  FLD    V1.Y
  FSUB   V2.Y
  FMUL   ST, ST
  FADD
  FLD    V1.Z
  FSUB   V2.Z
  FMUL   ST, ST
  FADD
  FSQRT
end;
{$ELSE}
begin
  Result := Sqrt((V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y) +
    (V1.Z - V2.Z) * (V1.Z - V2.Z));
end;
{$ENDIF}

function Vec3DistSqr(const V1, V2: TVector3): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FSUB   V2.X
  FMUL   ST, ST
  FLD    V1.Y
  FSUB   V2.Y
  FMUL   ST, ST
  FADD
  FLD    V1.Z
  FSUB   V2.Z
  FMUL   ST, ST
  FADD
end;
{$ELSE}
begin
  Result := (V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y) +
    (V1.Z - V2.Z) * (V1.Z - V2.Z);
end;
{$ENDIF}

function Vec3DistSquared(const V1, V2: TVector3): Single;
begin
  Result := (V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y) +
    (V1.Z - V2.Z) * (V1.Z - V2.Z);
end;

function Vec3Lerp(const V1, V2: TVector3; const T: Single): TVector3;
begin
  Result.X := V1.X + T * (V2.X - V1.X);
  Result.Y := V1.Y + T * (V2.Y - V1.Y);
  Result.Z := V1.Z + T * (V2.Z - V1.Z);
end;

function Vec3Maximize(const V1, V2: TVector3): TVector3;
begin
  Result := V1;
  if V2.X > Result.X then
    Result.X := V2.X;
  if V2.Y > Result.Y then
    Result.Y := V2.Y;
  if V2.Z > Result.Z then
    Result.Z := V2.Z;
end;

function Vec3Minimize(const V1, V2: TVector3): TVector3;
begin
  Result := V1;
  if V2.X < Result.X then
    Result.X := V2.X;
  if V2.Y < Result.Y then
    Result.Y := V2.Y;
  if V2.Z < Result.Z then
    Result.Z := V2.Z;
end;

procedure Vec3ScaleProc(var V: TVector3; const ScaleBy: Single);
begin
  V.X := V.X * ScaleBy;
  V.Y := V.Y * ScaleBy;
  V.Z := V.Z * ScaleBy;
end;

function Vec3Scale(const V: TVector3; const ScaleBy: Single): TVector3;
begin
  Result.X := V.X * ScaleBy;
  Result.Y := V.Y * ScaleBy;
  Result.Z := V.Z * ScaleBy;
end;

procedure Vec3NegateProc(var V: TVector3);
begin
  V.X := -V.X;
  V.Y := -V.Y;
  V.Z := -V.Z;
end;

function Vec3Negate(const V: TVector3): TVector3;
begin
  Result.X := -V.X;
  Result.Y := -V.Y;
  Result.Z := -V.Z;
end;

procedure Vec3NormalizeProc(var V: TVector3);
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FLD    V.Z
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   V.X
  FLD    ST
  FMUL   V.Y
  FSTP   V.Y
  FMUL   V.Z
  FSTP   V.Z
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y + V.Z * V.Z;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    V.X := V.X * InvLen;
    V.Y := V.Y * InvLen;
    V.Z := V.Z * InvLen;
  end
  else
    V := Vec3Zero;
end;
{$ENDIF}

function Vec3Normalize(const V: TVector3): TVector3;
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FLD    V.Z
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   Result.X
  FLD    ST
  FMUL   V.Y
  FSTP   Result.Y
  FMUL   V.Z
  FSTP   Result.Z
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y + V.Z * V.Z;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    Result.X := V.X * InvLen;
    Result.Y := V.Y * InvLen;
    Result.Z := V.Z * InvLen;
  end
  else
    Result := Vec3Zero;
end;
{$ENDIF}

function Vec3Transform(const V: TVector3; const M: TMatrix3): TVector3;
begin
  Result.X := V.X * M.Item11 + V.Y * M.Item21 + V.Z * M.Item31;
  Result.Y := V.X * M.Item12 + V.Y * M.Item22 + V.Z * M.Item32;
  Result.Z := V.X * M.Item13 + V.Y * M.Item23 + V.Z * M.Item33;
end;

function Vec3Average(const V1, V2: TVector3): TVector3;
begin
  Result.X := (V1.X + V2.X) * 0.5;
  Result.Y := (V1.Y + V2.Y) * 0.5;
  Result.Z := (V1.Z + V2.Z) * 0.5;
end;

function Vec3RotateX(const V: TVector3; const Angle : Single) : TVector3;
var
	Y0, Z0: Single;
	Radians: Single;
begin
	Y0 := V.Y;
	Z0 := V.Z;
	Radians := DegToRad(Angle);
  Result.X := V.X;
	Result.Y := (Y0 * Cos(Radians)) - (Z0 * Sin(Radians));
	Result.Z := (Y0 * Sin(Radians)) + (Z0 * Cos(Radians));
end;

function Vec3RotateY(const V: TVector3; const Angle : Single) : TVector3;
var
	X0, Z0: Single;
	Radians: Single;
begin
	X0 := V.X;
	Z0 := V.Z;
	Radians := DegToRad(Angle);
	Result.X := (X0 * Cos(Radians)) - (Z0 * Sin(Radians));
  Result.Y := V.Y;
	Result.Z := (X0 * Sin(Radians)) + (Z0 * Cos(Radians));
end;

function Vec3RotateZ(const V: TVector3; const Angle : Single) : TVector3;
var
	X0, Y0: Single;
	Radians: Single;
begin
	X0 := V.X;
	Y0 := V.Y;
	Radians := DegToRad(Angle);
	Result.X := (X0 * Cos(Radians)) - (Y0 * Sin(Radians));
	Result.Y := (X0 * Sin(Radians)) + (Y0 * Cos(Radians));
  Result.Z := V.Z;
end;

{* 4D Vector functions *}

function Vec4Add(const V1, V2: TVector4): TVector4;
begin
  Result.X := V1.X + V2.X;
  Result.Y := V1.Y + V2.Y;
  Result.Z := V1.Z + V2.Z;
  Result.W := V1.W + V2.W;
end;

function Vec4Subtract(const V1, V2: TVector4): TVector4;
begin
  Result.X := V1.X - V2.X;
  Result.Y := V1.Y - V2.Y;
  Result.Z := V1.Z - V2.Z;
  Result.W := V1.W - V2.W;
end;

function Vec4Multiply(const V1, V2: TVector4): TVector4;
begin
  Result.X := V1.X * V2.X;
  Result.Y := V1.Y * V2.Y;
  Result.Z := V1.Z * V2.Z;
  Result.W := V1.W * V2.W;
end;

function Vec4Divide(const V1, V2: TVector4): TVector4;
begin
  if V2.X <> 0 then Result.X := V1.X / V2.X else Result.X := 0;
  if V2.Y <> 0 then Result.Y := V1.Y / V2.Y else Result.Y := 0;
  if V2.Z <> 0 then Result.Z := V1.Z / V2.Z else Result.Z := 0;
  if V2.W <> 0 then Result.W := V1.W / V2.W else Result.W := 0;
end;

function Vec4Compare(const V1, V2: TVector4): Boolean;
begin
  Result :=
    (Abs(V1.X - V2.X) < SingleTolerance) and
    (Abs(V1.Y - V2.Y) < SingleTolerance) and
    (Abs(V1.Z - V2.Z) < SingleTolerance) and
    (Abs(V1.W - V2.W) < SingleTolerance);
end;

function Vec4Dot(const V1, V2: TVector4): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FMUL   V2.X
  FLD    V1.Y
  FMUL   V2.Y
  FADDP
  FLD    V1.Z
  FMUL   V2.Z
  FADDP
  FLD    V1.W
  FMUL   V2.W
  FADDP
end;
{$ELSE}
begin
  Result := V1.X * V2.X + V1.Y * V2.Y + V1.Z * V2.Z + V1.W * V2.W;
end;
{$ENDIF}

function Vec4Cross(const V1, V2: TVector4): TVector4;
begin
  Result.X := V1.Y * V2.Z - V1.Z * V2.Y;
  Result.Y := V1.Z * V2.X - V1.X * V2.Z;
  Result.Z := V1.X * V2.Y - V1.Y * V2.X;
  Result.W := 1;
end;

function Vec4Length(const V: TVector4): Single;
begin
  Result := Sqrt(V.X * V.X + V.Y * V.Y + V.Z * V.Z + V.W * V.W);
end;

function Vec4Dist(const V1, V2: TVector4): Single;
{$IFDEF USE_ASM}
asm
  FLD    V1.X
  FSUB   V2.X
  FMUL   ST, ST
  FLD    V1.Y
  FSUB   V2.Y
  FMUL   ST, ST
  FADD
  FLD    V1.Z
  FSUB   V2.Z
  FMUL   ST, ST
  FADD
  FLD    V1.W
  FSUB   V2.W
  FMUL   ST, ST
  FADD
  FSQRT
end;
{$ELSE}
begin
  Result := Sqrt((V1.X - V2.X) * (V1.X - V2.X) + (V1.Y - V2.Y) * (V1.Y - V2.Y) +
    (V1.Z - V2.Z) * (V1.Z - V2.Z) + (V1.W - V2.W) * (V1.W - V2.W));
end;
{$ENDIF}

function Vec4Lerp(const V1, V2: TVector4; const T: Single): TVector4;
begin
  Result.X := V1.X + T * (V2.X - V1.X);
  Result.Y := V1.Y + T * (V2.Y - V1.Y);
  Result.Z := V1.Z + T * (V2.Z - V1.Z);
  Result.W := V1.W + T * (V2.W - V1.W);
end;

function Vec4Maximize(const V1, V2: TVector4): TVector4;
begin
  Result := V1;
  if V2.X > Result.X then
    Result.X := V2.X;
  if V2.Y > Result.Y then
    Result.Y := V2.Y;
  if V2.Z > Result.Z then
    Result.Z := V2.Z;
  if V2.W > Result.W then
    Result.W := V2.W;
end;

function Vec4Minimize(const V1, V2: TVector4): TVector4;
begin
  Result := V1;
  if V2.X < Result.X then
    Result.X := V2.X;
  if V2.Y < Result.Y then
    Result.Y := V2.Y;
  if V2.Z < Result.Z then
    Result.Z := V2.Z;
  if V2.W < Result.W then
    Result.W := V2.W;
end;

procedure Vec4ScaleProc(var V: TVector4; const ScaleBy: Single);
begin
  V.X := V.X * ScaleBy;
  V.Y := V.Y * ScaleBy;
  V.Z := V.Z * ScaleBy;
  V.W := V.W * ScaleBy;
end;

function Vec4Scale(const V: TVector4; const ScaleBy: Single): TVector4;
begin
  Result.X := V.X * ScaleBy;
  Result.Y := V.Y * ScaleBy;
  Result.Z := V.Z * ScaleBy;
  Result.W := V.W * ScaleBy;
end;

procedure Vec4NegateProc(var V: TVector4);
begin
  V.X := -V.X;
  V.Y := -V.Y;
  V.Z := -V.Z;
  V.W := -V.W;
end;

function Vec4Negate(const V: TVector4): TVector4;
begin
  Result.X := -V.X;
  Result.Y := -V.Y;
  Result.Z := -V.Z;
  Result.W := -V.W;
end;

procedure Vec4NormalizeProc(var V: TVector4);
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FLD    V.Z
  FMUL   ST, ST
  FADD
  FLD    V.W
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   V.X
  FLD    ST
  FMUL   V.Y
  FSTP   V.Y
  FLD    ST
  FMUL   V.Z
  FSTP   V.Z
  FMUL   V.W
  FSTP   V.W
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y + V.Z * V.Z + V.W * V.W;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    V.X := V.X * InvLen;
    V.Y := V.Y * InvLen;
    V.Z := V.Z * InvLen;
    V.W := V.W * InvLen;
  end
  else
    V := Vec4Zero;
end;
{$ENDIF}

function Vec4Normalize(const V: TVector4): TVector4;
{$IFDEF USE_ASM}
asm
  FLD    V.X
  FMUL   ST, ST
  FLD    V.Y
  FMUL   ST, ST
  FADD
  FLD    V.Z
  FMUL   ST, ST
  FADD
  FLD    V.W
  FMUL   ST, ST
  FADD
  FSQRT
  FLD1
  FDIVR
  FLD    ST
  FMUL   V.X
  FSTP   Result.X
  FLD    ST
  FMUL   V.Y
  FSTP   Result.Y
  FLD    ST
  FMUL   V.Z
  FSTP   Result.Z
  FMUL   V.W
  FSTP   Result.W
end;
{$ELSE}
var
  Len: Single;
  InvLen: Single;
begin
  Len := V.X * V.X + V.Y * V.Y + V.Z * V.Z + V.W * V.W;
  if Len <> 0 then
  begin
    InvLen := 1 / Sqrt(Len);
    Result.X := V.X * InvLen;
    Result.Y := V.Y * InvLen;
    Result.Z := V.Z * InvLen;
    Result.W := V.W * InvLen;
  end
  else
    Result := Vec4Zero;
end;
{$ENDIF}

function Vec4Transform(const V: TVector4; const M: TMatrix4): TVector4;
begin
  Result.X := V.X * M.Item11 + V.Y * M.Item21 + V.Z * M.Item31 + V.W * M.Item41;
  Result.Y := V.X * M.Item12 + V.Y * M.Item22 + V.Z * M.Item32 + V.W * M.Item42;
  Result.Z := V.X * M.Item13 + V.Y * M.Item23 + V.Z * M.Item33 + V.W * M.Item43;
  Result.W := V.X * M.Item14 + V.Y * M.Item24 + V.Z * M.Item34 + V.W * M.Item44;
end;


{*************************************************************************}
{                            Matrix functions                             }
{*************************************************************************}


procedure Matrix3(const Singles: array of Single; var M: TMatrix3);
begin
  M.Singles[0] := Singles[0];
  M.Singles[1] := Singles[1];
  M.Singles[2] := Singles[2];
  M.Singles[3] := Singles[3];
  M.Singles[4] := Singles[4];
  M.Singles[5] := Singles[5];
  M.Singles[6] := Singles[6];
  M.Singles[7] := Singles[7];
  M.Singles[8] := Singles[8];
end;

procedure Matrix3(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; var M: TMatrix3);
begin
  M.Arr[0, 0] := Row1[0];
  M.Arr[0, 1] := Row1[1];
  M.Arr[0, 2] := Row1[2];
  M.Arr[1, 0] := Row2[0];
  M.Arr[1, 1] := Row2[1];
  M.Arr[1, 2] := Row2[2];
  M.Arr[2, 0] := Row3[0];
  M.Arr[2, 1] := Row3[1];
  M.Arr[2, 2] := Row3[2];
end;

procedure Matrix4(const Singles: array of Single; var M: TMatrix4);
begin
  M.Singles[0] := Singles[0];
  M.Singles[1] := Singles[1];
  M.Singles[2] := Singles[2];
  M.Singles[3] := Singles[3];
  M.Singles[4] := Singles[4];
  M.Singles[5] := Singles[5];
  M.Singles[6] := Singles[6];
  M.Singles[7] := Singles[7];
  M.Singles[8] := Singles[8];
  M.Singles[9] := Singles[9];
  M.Singles[10] := Singles[10];
  M.Singles[11] := Singles[11];
  M.Singles[12] := Singles[12];
  M.Singles[13] := Singles[13];
  M.Singles[14] := Singles[14];
  M.Singles[15] := Singles[15];
end;

procedure Matrix4(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; const Row4: array of Single; var M: TMatrix4);
begin
  M.Arr[0, 0] := Row1[0];
  M.Arr[0, 1] := Row1[1];
  M.Arr[0, 2] := Row1[2];
  M.Arr[0, 3] := Row1[3];
  M.Arr[1, 0] := Row2[0];
  M.Arr[1, 1] := Row2[1];
  M.Arr[1, 2] := Row2[2];
  M.Arr[1, 3] := Row2[3];
  M.Arr[2, 0] := Row3[0];
  M.Arr[2, 1] := Row3[1];
  M.Arr[2, 2] := Row3[2];
  M.Arr[2, 3] := Row3[3];
  M.Arr[3, 0] := Row4[0];
  M.Arr[3, 1] := Row4[1];
  M.Arr[3, 2] := Row4[2];
  M.Arr[3, 3] := Row4[3];
end;

function Matrix3(const Singles: array of Single): TMatrix3;
begin
  Result.Singles[0] := Singles[0];
  Result.Singles[1] := Singles[1];
  Result.Singles[2] := Singles[2];
  Result.Singles[3] := Singles[3];
  Result.Singles[4] := Singles[4];
  Result.Singles[5] := Singles[5];
  Result.Singles[6] := Singles[6];
  Result.Singles[7] := Singles[7];
  Result.Singles[8] := Singles[8];
end;

function Matrix3(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single): TMatrix3;
begin
  Result.Arr[0, 0] := Row1[0];
  Result.Arr[0, 1] := Row1[1];
  Result.Arr[0, 2] := Row1[2];
  Result.Arr[1, 0] := Row2[0];
  Result.Arr[1, 1] := Row2[1];
  Result.Arr[1, 2] := Row2[2];
  Result.Arr[2, 0] := Row3[0];
  Result.Arr[2, 1] := Row3[1];
  Result.Arr[2, 2] := Row3[2];
end;

function Matrix4(const Singles: array of Single): TMatrix4;
begin
  Result.Singles[0] := Singles[0];
  Result.Singles[1] := Singles[1];
  Result.Singles[2] := Singles[2];
  Result.Singles[3] := Singles[3];
  Result.Singles[4] := Singles[4];
  Result.Singles[5] := Singles[5];
  Result.Singles[6] := Singles[6];
  Result.Singles[7] := Singles[7];
  Result.Singles[8] := Singles[8];
  Result.Singles[9] := Singles[9];
  Result.Singles[10] := Singles[10];
  Result.Singles[11] := Singles[11];
  Result.Singles[12] := Singles[12];
  Result.Singles[13] := Singles[13];
  Result.Singles[14] := Singles[14];
  Result.Singles[15] := Singles[15];
end;

function Matrix4(const Row1: array of Single; const Row2: array of Single;
  const Row3: array of Single; const Row4: array of Single): TMatrix4;
begin
  Result.Arr[0, 0] := Row1[0];
  Result.Arr[0, 1] := Row1[1];
  Result.Arr[0, 2] := Row1[2];
  Result.Arr[0, 3] := Row1[3];
  Result.Arr[1, 0] := Row2[0];
  Result.Arr[1, 1] := Row2[1];
  Result.Arr[1, 2] := Row2[2];
  Result.Arr[1, 3] := Row2[3];
  Result.Arr[2, 0] := Row3[0];
  Result.Arr[2, 1] := Row3[1];
  Result.Arr[2, 2] := Row3[2];
  Result.Arr[2, 3] := Row3[3];
  Result.Arr[3, 0] := Row4[0];
  Result.Arr[3, 1] := Row4[1];
  Result.Arr[3, 2] := Row4[2];
  Result.Arr[3, 3] := Row4[3];
end;

{* 3x3 Matrix functions *}

function Mat3Add(const M1, M2: TMatrix3): TMatrix3;
begin
  Result.Singles[0] := M1.Singles[0] + M2.Singles[0];
  Result.Singles[1] := M1.Singles[1] + M2.Singles[1];
  Result.Singles[2] := M1.Singles[2] + M2.Singles[2];
  Result.Singles[3] := M1.Singles[3] + M2.Singles[3];
  Result.Singles[4] := M1.Singles[4] + M2.Singles[4];
  Result.Singles[5] := M1.Singles[5] + M2.Singles[5];
  Result.Singles[6] := M1.Singles[6] + M2.Singles[6];
  Result.Singles[7] := M1.Singles[7] + M2.Singles[7];
  Result.Singles[8] := M1.Singles[8] + M2.Singles[8];
end;

function Mat3Subtract(const M1, M2: TMatrix3): TMatrix3;
begin
  Result.Singles[0] := M1.Singles[0] - M2.Singles[0];
  Result.Singles[1] := M1.Singles[1] - M2.Singles[1];
  Result.Singles[2] := M1.Singles[2] - M2.Singles[2];
  Result.Singles[3] := M1.Singles[3] - M2.Singles[3];
  Result.Singles[4] := M1.Singles[4] - M2.Singles[4];
  Result.Singles[5] := M1.Singles[5] - M2.Singles[5];
  Result.Singles[6] := M1.Singles[6] - M2.Singles[6];
  Result.Singles[7] := M1.Singles[7] - M2.Singles[7];
  Result.Singles[8] := M1.Singles[8] - M2.Singles[8];
end;

function Mat3Multiply(const M1, M2: TMatrix3): TMatrix3;
begin
  Result.Arr[0, 0] := M1.Arr[0, 0] * M2.Arr[0, 0] + M1.Arr[0, 1] * M2.Arr[1, 0]
    + M1.Arr[0, 2] * M2.Arr[2, 0];
  Result.Arr[0, 1] := M1.Arr[0, 0] * M2.Arr[0, 1] + M1.Arr[0, 1] * M2.Arr[1, 1]
    + M1.Arr[0, 2] * M2.Arr[2, 1];
  Result.Arr[0, 2] := M1.Arr[0, 0] * M2.Arr[0, 2] + M1.Arr[0, 1] * M2.Arr[1, 2]
    + M1.Arr[0, 2] * M2.Arr[2, 2];
  Result.Arr[1, 0] := M1.Arr[1, 0] * M2.Arr[0, 0] + M1.Arr[1, 1] * M2.Arr[1, 0]
    + M1.Arr[1, 2] * M2.Arr[2, 0];
  Result.Arr[1, 1] := M1.Arr[1, 0] * M2.Arr[0, 1] + M1.Arr[1, 1] * M2.Arr[1, 1]
    + M1.Arr[1, 2] * M2.Arr[2, 1];
  Result.Arr[1, 2] := M1.Arr[1, 0] * M2.Arr[0, 2] + M1.Arr[1, 1] * M2.Arr[1, 2]
    + M1.Arr[1, 2] * M2.Arr[2, 2];
  Result.Arr[2, 0] := M1.Arr[2, 0] * M2.Arr[0, 0] + M1.Arr[2, 1] * M2.Arr[1, 0]
    + M1.Arr[2, 2] * M2.Arr[2, 0];
  Result.Arr[2, 1] := M1.Arr[2, 0] * M2.Arr[0, 1] + M1.Arr[2, 1] * M2.Arr[1, 1]
    + M1.Arr[2, 2] * M2.Arr[2, 1];
  Result.Arr[2, 2] := M1.Arr[2, 0] * M2.Arr[0, 2] + M1.Arr[2, 1] * M2.Arr[1, 2]
    + M1.Arr[2, 2] * M2.Arr[2, 2];
end;

function Mat3ScalarMultiply(const M: TMatrix3; const F: Single): TMatrix3;
begin
  Result.Singles[0] := M.Singles[0] * F;
  Result.Singles[1] := M.Singles[1] * F;
  Result.Singles[2] := M.Singles[2] * F;
  Result.Singles[3] := M.Singles[3] * F;
  Result.Singles[4] := M.Singles[4] * F;
  Result.Singles[5] := M.Singles[5] * F;
  Result.Singles[6] := M.Singles[6] * F;
  Result.Singles[7] := M.Singles[7] * F;
  Result.Singles[8] := M.Singles[8] * F;
end;

function Mat3Compare(const M1, M2: TMatrix3): Boolean;
begin
  Result :=
    (Abs(M1.Singles[0] - M2.Singles[0]) < SingleTolerance) and
    (Abs(M1.Singles[1] - M2.Singles[1]) < SingleTolerance) and
    (Abs(M1.Singles[2] - M2.Singles[2]) < SingleTolerance) and
    (Abs(M1.Singles[3] - M2.Singles[3]) < SingleTolerance) and
    (Abs(M1.Singles[4] - M2.Singles[4]) < SingleTolerance) and
    (Abs(M1.Singles[5] - M2.Singles[5]) < SingleTolerance) and
    (Abs(M1.Singles[6] - M2.Singles[6]) < SingleTolerance) and
    (Abs(M1.Singles[7] - M2.Singles[7]) < SingleTolerance) and
    (Abs(M1.Singles[8] - M2.Singles[8]) < SingleTolerance);
end;

function Mat3Transpose(const M: TMatrix3): TMatrix3;
begin
  Result.Arr[0, 0] := M.Arr[0, 0];
  Result.Arr[0, 1] := M.Arr[1, 0];
  Result.Arr[0, 2] := M.Arr[2, 0];
  Result.Arr[1, 0] := M.Arr[0, 1];
  Result.Arr[1, 1] := M.Arr[1, 1];
  Result.Arr[1, 2] := M.Arr[2, 1];
  Result.Arr[2, 0] := M.Arr[0, 2];
  Result.Arr[2, 1] := M.Arr[1, 2];
  Result.Arr[2, 2] := M.Arr[2, 2];
end;

function Mat3Determinant(const M: TMatrix3): Single;
begin
  Result :=
    M.Arr[0, 0] * (M.Arr[1, 1] * M.Arr[2, 2] - M.Arr[2, 1] * M.Arr[1, 2])
    - M.Arr[0, 1] * (M.Arr[1, 0] * M.Arr[2, 2] - M.Arr[2, 0] * M.Arr[1, 2])
    + M.Arr[0, 2] * (M.Arr[1, 0] * M.Arr[2, 1] - M.Arr[2, 0] * M.Arr[1, 1]);
end;

function Mat3Inverse(const M: TMatrix3): TMatrix3;
var
  Det: Single;
begin
  Det := Mat3Determinant(M);
  if Abs(Det) < SingleTolerance then
  begin
    Result := Mat3Identity;
    Exit;
  end;
  Result.Arr[0, 0] := M.Arr[1, 1] * M.Arr[2, 2] - M.Arr[1, 2] * M.Arr[2, 1] / Det;
  Result.Arr[0, 1] := -(M.Arr[0, 1] * M.Arr[2, 2] - M.Arr[2, 1] * M.Arr[0, 2]) / Det;
  Result.Arr[0, 2] := M.Arr[0, 1] * M.Arr[1, 2] - M.Arr[1, 1] * M.Arr[0, 2] / Det;
  Result.Arr[1, 0] := -(M.Arr[1, 0] * M.Arr[2, 2] - M.Arr[1, 2] * M.Arr[2, 0]) / Det;
  Result.Arr[1, 1] := M.Arr[0, 0] * M.Arr[2, 2] - M.Arr[2, 0] * M.Arr[0, 2] / Det;
  Result.Arr[1, 2] := -(M.Arr[0, 0] * M.Arr[1, 2] - M.Arr[1, 0] * M.Arr[0, 2]) / Det;
  Result.Arr[2, 0] := M.Arr[1, 0] * M.Arr[2, 1] - M.Arr[2, 0] * M.Arr[1, 1] / Det;
  Result.Arr[2, 1] := -(M.Arr[0, 0] * M.Arr[2, 1] - M.Arr[2, 0] * M.Arr[0, 1]) / Det;
  Result.Arr[2, 2] := M.Arr[0, 0] * M.Arr[1, 1] - M.Arr[0, 1] * M.Arr[1, 0] / Det;
end;

{* 4x4 Matrix functions *}

function Mat4Add(const M1, M2: TMatrix4): TMatrix4;
begin
  Result.Singles[0] := M1.Singles[0] + M2.Singles[0];
  Result.Singles[1] := M1.Singles[1] + M2.Singles[1];
  Result.Singles[2] := M1.Singles[2] + M2.Singles[2];
  Result.Singles[3] := M1.Singles[3] + M2.Singles[3];
  Result.Singles[4] := M1.Singles[4] + M2.Singles[4];
  Result.Singles[5] := M1.Singles[5] + M2.Singles[5];
  Result.Singles[6] := M1.Singles[6] + M2.Singles[6];
  Result.Singles[7] := M1.Singles[7] + M2.Singles[7];
  Result.Singles[8] := M1.Singles[8] + M2.Singles[8];
  Result.Singles[9] := M1.Singles[9] + M2.Singles[9];
  Result.Singles[10] := M1.Singles[10] + M2.Singles[10];
  Result.Singles[11] := M1.Singles[11] + M2.Singles[11];
  Result.Singles[12] := M1.Singles[12] + M2.Singles[12];
  Result.Singles[13] := M1.Singles[13] + M2.Singles[13];
  Result.Singles[14] := M1.Singles[14] + M2.Singles[14];
  Result.Singles[15] := M1.Singles[15] + M2.Singles[15];
end;

function Mat4Subtract(const M1, M2: TMatrix4): TMatrix4;
begin
  Result.Singles[0] := M1.Singles[0] - M2.Singles[0];
  Result.Singles[1] := M1.Singles[1] - M2.Singles[1];
  Result.Singles[2] := M1.Singles[2] - M2.Singles[2];
  Result.Singles[3] := M1.Singles[3] - M2.Singles[3];
  Result.Singles[4] := M1.Singles[4] - M2.Singles[4];
  Result.Singles[5] := M1.Singles[5] - M2.Singles[5];
  Result.Singles[6] := M1.Singles[6] - M2.Singles[6];
  Result.Singles[7] := M1.Singles[7] - M2.Singles[7];
  Result.Singles[8] := M1.Singles[8] - M2.Singles[8];
  Result.Singles[9] := M1.Singles[9] - M2.Singles[9];
  Result.Singles[10] := M1.Singles[10] - M2.Singles[10];
  Result.Singles[11] := M1.Singles[11] - M2.Singles[11];
  Result.Singles[12] := M1.Singles[12] - M2.Singles[12];
  Result.Singles[13] := M1.Singles[13] - M2.Singles[13];
  Result.Singles[14] := M1.Singles[14] - M2.Singles[14];
  Result.Singles[15] := M1.Singles[15] - M2.Singles[15];
end;

function Mat4Multiply(const M1, M2: TMatrix4): TMatrix4;
begin
  Result.Arr[0, 0] := M1.Arr[0, 0] * M2.Arr[0, 0] + M1.Arr[0, 1] * M2.Arr[1, 0]
    + M1.Arr[0, 2] * M2.Arr[2, 0] + M1.Arr[0, 3] * M2.Arr[3, 0];
  Result.Arr[0, 1] := M1.Arr[0, 0] * M2.Arr[0, 1] + M1.Arr[0, 1] * M2.Arr[1, 1]
    + M1.Arr[0, 2] * M2.Arr[2, 1] + M1.Arr[0, 3] * M2.Arr[3, 1];
  Result.Arr[0, 2] := M1.Arr[0, 0] * M2.Arr[0, 2] + M1.Arr[0, 1] * M2.Arr[1, 2]
    + M1.Arr[0, 2] * M2.Arr[2, 2] + M1.Arr[0, 3] * M2.Arr[3, 2];
  Result.Arr[0, 3] := M1.Arr[0, 0] * M2.Arr[0, 3] + M1.Arr[0, 1] * M2.Arr[1, 3]
    + M1.Arr[0, 2] * M2.Arr[2, 3] + M1.Arr[0, 3] * M2.Arr[3, 3];
  Result.Arr[1, 0] := M1.Arr[1, 0] * M2.Arr[0, 0] + M1.Arr[1, 1] * M2.Arr[1, 0]
    + M1.Arr[1, 2] * M2.Arr[2, 0] + M1.Arr[1, 3] * M2.Arr[3, 0];
  Result.Arr[1, 1] := M1.Arr[1, 0] * M2.Arr[0, 1] + M1.Arr[1, 1] * M2.Arr[1, 1]
    + M1.Arr[1, 2] * M2.Arr[2, 1] + M1.Arr[1, 3] * M2.Arr[3, 1];
  Result.Arr[1, 2] := M1.Arr[1, 0] * M2.Arr[0, 2] + M1.Arr[1, 1] * M2.Arr[1, 2]
    + M1.Arr[1, 2] * M2.Arr[2, 2] + M1.Arr[1, 3] * M2.Arr[3, 2];
  Result.Arr[1, 3] := M1.Arr[1, 0] * M2.Arr[0, 3] + M1.Arr[1, 1] * M2.Arr[1, 3]
    + M1.Arr[1, 2] * M2.Arr[2, 3] + M1.Arr[1, 3] * M2.Arr[3, 3];
  Result.Arr[2, 0] := M1.Arr[2, 0] * M2.Arr[0, 0] + M1.Arr[2, 1] * M2.Arr[1, 0]
    + M1.Arr[2, 2] * M2.Arr[2, 0] + M1.Arr[2, 3] * M2.Arr[3, 0];
  Result.Arr[2, 1] := M1.Arr[2, 0] * M2.Arr[0, 1] + M1.Arr[2, 1] * M2.Arr[1, 1]
    + M1.Arr[2, 2] * M2.Arr[2, 1] + M1.Arr[2, 3] * M2.Arr[3, 1];
  Result.Arr[2, 2] := M1.Arr[2, 0] * M2.Arr[0, 2] + M1.Arr[2, 1] * M2.Arr[1, 2]
    + M1.Arr[2, 2] * M2.Arr[2, 2] + M1.Arr[2, 3] * M2.Arr[3, 2];
  Result.Arr[2, 3] := M1.Arr[2, 0] * M2.Arr[0, 3] + M1.Arr[2, 1] * M2.Arr[1, 3]
    + M1.Arr[2, 2] * M2.Arr[2, 3] + M1.Arr[2, 3] * M2.Arr[3, 3];
  Result.Arr[3, 0] := M1.Arr[3, 0] * M2.Arr[0, 0] + M1.Arr[3, 1] * M2.Arr[1, 0]
    + M1.Arr[3, 2] * M2.Arr[2, 0] + M1.Arr[3, 3] * M2.Arr[3, 0];
  Result.Arr[3, 1] := M1.Arr[3, 0] * M2.Arr[0, 1] + M1.Arr[3, 1] * M2.Arr[1, 1]
    + M1.Arr[3, 2] * M2.Arr[2, 1] + M1.Arr[3, 3] * M2.Arr[3, 1];
  Result.Arr[3, 2] := M1.Arr[3, 0] * M2.Arr[0, 2] + M1.Arr[3, 1] * M2.Arr[1, 2]
    + M1.Arr[3, 2] * M2.Arr[2, 2] + M1.Arr[3, 3] * M2.Arr[3, 2];
  Result.Arr[3, 3] := M1.Arr[3, 0] * M2.Arr[0, 3] + M1.Arr[3, 1] * M2.Arr[1, 3]
    + M1.Arr[3, 2] * M2.Arr[2, 3] + M1.Arr[3, 3] * M2.Arr[3, 3];
end;

function Mat4ScalarMultiply(const M: TMatrix4; const F: Single): TMatrix4;
begin
  Result.Singles[0] := M.Singles[0] * F;
  Result.Singles[1] := M.Singles[1] * F;
  Result.Singles[2] := M.Singles[2] * F;
  Result.Singles[3] := M.Singles[3] * F;
  Result.Singles[4] := M.Singles[4] * F;
  Result.Singles[5] := M.Singles[5] * F;
  Result.Singles[6] := M.Singles[6] * F;
  Result.Singles[7] := M.Singles[7] * F;
  Result.Singles[8] := M.Singles[8] * F;
  Result.Singles[9] := M.Singles[9] * F;
  Result.Singles[10] := M.Singles[10] * F;
  Result.Singles[11] := M.Singles[11] * F;
  Result.Singles[12] := M.Singles[12] * F;
  Result.Singles[13] := M.Singles[13] * F;
  Result.Singles[14] := M.Singles[14] * F;
  Result.Singles[15] := M.Singles[15] * F;
end;

function Mat4Compare(const M1, M2: TMatrix4): Boolean;
begin
  Result :=
    (Abs(M1.Singles[0] - M2.Singles[0]) < SingleTolerance) and
    (Abs(M1.Singles[1] - M2.Singles[1]) < SingleTolerance) and
    (Abs(M1.Singles[2] - M2.Singles[2]) < SingleTolerance) and
    (Abs(M1.Singles[3] - M2.Singles[3]) < SingleTolerance) and
    (Abs(M1.Singles[4] - M2.Singles[4]) < SingleTolerance) and
    (Abs(M1.Singles[5] - M2.Singles[5]) < SingleTolerance) and
    (Abs(M1.Singles[6] - M2.Singles[6]) < SingleTolerance) and
    (Abs(M1.Singles[7] - M2.Singles[7]) < SingleTolerance) and
    (Abs(M1.Singles[9] - M2.Singles[9]) < SingleTolerance) and
    (Abs(M1.Singles[10] - M2.Singles[10]) < SingleTolerance) and
    (Abs(M1.Singles[11] - M2.Singles[11]) < SingleTolerance) and
    (Abs(M1.Singles[12] - M2.Singles[12]) < SingleTolerance) and
    (Abs(M1.Singles[13] - M2.Singles[13]) < SingleTolerance) and
    (Abs(M1.Singles[14] - M2.Singles[14]) < SingleTolerance) and
    (Abs(M1.Singles[15] - M2.Singles[15]) < SingleTolerance);
end;

function Mat4Transpose(const M: TMatrix4): TMatrix4;
begin
  Result.Arr[0, 0] := M.Arr[0, 0];
  Result.Arr[0, 1] := M.Arr[1, 0];
  Result.Arr[0, 2] := M.Arr[2, 0];
  Result.Arr[0, 3] := M.Arr[3, 0];
  Result.Arr[1, 0] := M.Arr[0, 1];
  Result.Arr[1, 1] := M.Arr[1, 1];
  Result.Arr[1, 2] := M.Arr[2, 1];
  Result.Arr[1, 3] := M.Arr[3, 1];
  Result.Arr[2, 0] := M.Arr[0, 2];
  Result.Arr[2, 1] := M.Arr[1, 2];
  Result.Arr[2, 2] := M.Arr[2, 2];
  Result.Arr[2, 3] := M.Arr[3, 2];
  Result.Arr[3, 0] := M.Arr[0, 3];
  Result.Arr[3, 1] := M.Arr[1, 3];
  Result.Arr[3, 2] := M.Arr[2, 3];
  Result.Arr[3, 3] := M.Arr[3, 3];
end;

function MatDetInternal(const A1, A2, A3, B1, B2, B3, C1, C2, C3: Single): Single;
begin
  Result := A1 * (B2 * C3 - B3 * C2)
    - B1 * (A2 * C3 - A3 * C2)
    + C1 * (A2 * B3 - A3 * B2);
end;

function Mat4Determinant(const M: TMatrix4): Single;
begin
  Result := M.Arr[0, 0] *
    MatDetInternal(M.Arr[1, 1], M.Arr[2, 1], M.Arr[3, 1],
                   M.Arr[1, 2], M.Arr[2, 2], M.Arr[3, 2],
                   M.Arr[1, 3], M.Arr[2, 3], M.Arr[3, 3]) - M.Arr[0, 1] *
    MatDetInternal(M.Arr[1, 0], M.Arr[2, 0], M.Arr[3, 0],
                   M.Arr[1, 2], M.Arr[2, 2], M.Arr[3, 2],
                   M.Arr[1, 3], M.Arr[2, 3], M.Arr[3, 3]) + M.Arr[0, 2] *
    MatDetInternal(M.Arr[1, 0], M.Arr[2, 0], M.Arr[3, 0],
                   M.Arr[1, 1], M.Arr[2, 1], M.Arr[3, 1],
                   M.Arr[1, 3], M.Arr[2, 3], M.Arr[3, 3]) - M.Arr[0, 3] *
    MatDetInternal(M.Arr[1, 0], M.Arr[2, 0], M.Arr[3, 0],
                   M.Arr[1, 1], M.Arr[2, 1], M.Arr[3, 1],
                   M.Arr[1, 2], M.Arr[2, 2], M.Arr[3, 2]);
end;

function MatGet3x3SubInternal(const M: TMatrix4; R, C: LongInt): TMatrix3;
var
  DI, DJ, SI, SJ: LongInt;
begin
  for DI := 0 to 2 do
  begin
    for DJ := 0 to 2 do
    begin
      if DI >= R then
        SI := DI + 1
      else
        SI := DI;
      if DJ >= C then
        SJ := DJ + 1
      else
        SJ := DJ;
      Result.Arr[DI, DJ] := M.Arr[SI, SJ];
    end;
  end;
end;

function Mat4Inverse(const M: TMatrix4): TMatrix4;
var
  Det: Single;
  Tmp: TMatrix3;
  I, J, Sign: LongInt;
begin
  Det := Mat4Determinant(M);
  if Abs(Det) < SingleTolerance then
  begin
    Result := Mat4Identity;
    Exit;
  end;
  for I := 0 to 3 do
    for J := 0 to 3 do
    begin
      Sign := 1 - ((I + J) mod 2) * 2;
      Tmp := MatGet3x3SubInternal(M, I, J);
      Result.Arr[J, I] := Mat3Determinant(Tmp) * Sign / Det;
    end;
end;


{*************************************************************************}
{                            Quaternion functions                         }
{*************************************************************************}

function Quaternion(const X, Y, Z, W: Single): TQuaternion;
begin
  Result.X := X;
  Result.Y := Y;
  Result.Z := Z;
  Result.W := W;
end;

function QuatToMatrix(const Q :TQuaternion): TMatrix4;
var
  Norm, S, XS, WX, X, Y, Z, YS, WY, XY, YZ, ZS, WZ, XZ: Single;
begin
  Norm := Sqr(Q.X) + Sqr(Q.Y) + Sqr(Q.Z) + Sqr(Q.W);
  if Norm > 0.0 then
    S := 2 / Norm
  else
    S := 0;
  XS := Q.X * S;
  YS := Q.Y * S;
  ZS := Q.Z * S;
  WX := Q.W * XS;
  WY := Q.W * YS;
  WZ := Q.W * ZS;
  X := Q.X * XS;
  XY := Q.X * YS;
  XZ := Q.X * ZS;
  Y := Q.Y * YS;
  YZ := Q.Y * ZS;
  Z := Q.Z * ZS;
  Result.Arr[0, 0] := 1 - (Y + Z);
  Result.Arr[0, 1] := XY + WZ;
  Result.Arr[0, 2] := XZ - WY;
  Result.Arr[1, 0] := XY - WZ;
  Result.Arr[1, 1] := 1.0 - (X + Z);
  Result.Arr[1, 2] := YZ + WX;
  Result.Arr[2, 0] := XZ + WY;
  Result.Arr[2, 1] := YZ - WX;
  Result.Arr[2, 2] := 1.0 - (X + Y);
  Result.Arr[3, 0] := 0;
  Result.Arr[3, 1] := 0;
  Result.Arr[3, 2] := 0;
  Result.Arr[0, 3] := 0;
  Result.Arr[1, 3] := 0;
  Result.Arr[2, 3] := 0.0;
  Result.Arr[3, 3] := 1.0;
end;

function QuatMultiply(const Q1, Q2: TQuaternion): TQuaternion;
begin
  Result.W := Q1.W * Q2.W - Q1.X * Q2.X - Q1.Y * Q2.Y - Q1.Z * Q2.Z;
  Result.X := Q1.W * Q2.X + Q1.X * Q2.W + Q1.Y * Q2.Z - Q1.Z * Q2.Y;
  Result.Y := Q1.W * Q2.Y + Q1.Y * Q2.W + Q1.Z * Q2.X - Q1.X * Q2.Z;
  Result.Z := Q1.W * Q2.Z + Q1.Z * Q2.W + Q1.X * Q2.Y - Q1.Y * Q2.X;
end;

function QuatConjugate(const Q: TQuaternion): TQuaternion;
begin
  Result.X := -Q.X;
  Result.Y := -Q.Y;
  Result.Z := -Q.Z;
  Result.W := Q.W;
end;

{*************************************************************************}
{                              Plane functions                            }
{*************************************************************************}

function Plane(const A, B, C, D: Single): TPlane;
begin
  Result.A := A;
  Result.B := B;
  Result.C := C;
  Result.D := D;
end;

function Plane(const Normal: TVector3; const D: Single): TPlane;
begin
  Result.Normal := Normal;
  Result.D := D;
end;

procedure PlaneNormalizeProc(var Plane: TPlane);
var
  Magnitude: Single;
begin
  Magnitude := Sqrt(Plane.A * Plane.A +
    Plane.B * Plane.B + Plane.C * Plane.C);

  Plane.A := Plane.A / Magnitude;
  Plane.B := Plane.B / Magnitude;
  Plane.C := Plane.C / Magnitude;
  Plane.D := Plane.D / Magnitude;
end;

function PlaneNormalize(const Plane: TPlane): TPlane;
var
  Magnitude: Single;
begin
  Magnitude := Sqrt(Plane.A * Plane.A +
    Plane.B * Plane.B + Plane.C * Plane.C);

  Result.A := Plane.A / Magnitude;
  Result.B := Plane.B / Magnitude;
  Result.C := Plane.C / Magnitude;
  Result.D := Plane.D / Magnitude;
end;

function PlaneDistanceToPoint(const Plane: TPlane; const Point: TVector3): Single;
begin
  Result := Vec3Dot(Plane.Normal, Point) + Plane.D;
end;

end.

